/**
 * @ Author: luoqi
 * @ Create Time: 2025-01-07 22:54
 * @ Modified by: luoqi
 * @ Modified time: 2025-04-11 19:30
 * @ Description:
 */

#include <bsp.h>
#include <math.h>
#include "focc.h"
#include "focd.h"
#include "../dev/qdrive.h"
#include "../qshell/qterm.h"
#include "../controller/trajc_plan.h"

#define QLOG_TAG "FOCC"
#include "../log/qlog.h"   

static QCliCmd cmd_focc1, cmd_focc2;
static QCliCmd cmd_focd1, cmd_focd2;

static QDrive *drv;
static FoccObj *foccobj;
static Focc focc;

// static TrajcSPlan _trajc_s;
static int cmd_focc_hdl(int argc, char **argv);
static int cmd_focc1_hdl(int argc, char **argv)
{
    foccobj = &focc.focc1;
    return cmd_focc_hdl(argc, argv);
}

static int cmd_focc2_hdl(int argc, char **argv)
{
    foccobj = &focc.focc2;
    return cmd_focc_hdl(argc, argv);
}

static int cmd_focd_hdl(int argc, char **argv);
static int cmd_focd1_hdl(int argc, char **argv)
{
    foccobj = &focc.focc1;
    return cmd_focd_hdl(argc, argv);
}

static int cmd_focd2_hdl(int argc, char **argv)
{
    foccobj = &focc.focc2;
    return cmd_focd_hdl(argc, argv);
}

typedef enum {
    FOCC_DLOOP_NONE = 0,
    FOCC_DLOOP_IQD,
    FOCC_DLOOP_V,
    FOCC_DLOOP_P
} FoccDLoop;

typedef struct {
    FoccDLoop dloop;
    FocDebugger focd;
} FoccDebugger;
static FoccDebugger foccd1, foccd2;

static int foccd_init(FoccDebugger *foccd, int (dump)(const uint8_t *data, uint32_t len))
{
    foccd->dloop = FOCC_DLOOP_NONE;
    foccd->focd.dump = dump;
    return focd_init(&foccd->focd, 1000, 1, dump);
}

static int foccd_output(FoccDebugger *foccd, FoccDLoop loop, qfp_t *output)
{
    if(foccd->dloop != loop) {
        return 0;
    } else {
        return focd_signal_output(&foccd->focd, output);
    }
}

static int dump_data(const uint8_t *data, uint32_t len, FoccDebugger *foccd, const char *foc_name)
{
    if(foccd->focd.ena) {
        QLOG_ERR("%s is debugging, can't dump data", foc_name);
        return -1;
    }
    if(len > 28) {
        QLOG_ERR("dump data too long, %d", len);
        return -1;
    }
    static uint8_t buf[32] = { 0 };
    memcpy(buf, data, len);
    buf[len] = 0x00;
    buf[len + 1] = 0x00;
    buf[len + 2] = 0x80;
    buf[len + 3] = 0x7f;

    int sz = bsp_usb_send(buf, len + 4);
    if(sz != len + 4) {
        QLOG_ERR("dump failed, %d", sz);
        foccd->focd.is_dump = false;
    }
    return sz;
}

static int dump1(const uint8_t *data, uint32_t len)
{
    return dump_data(data, len, &foccd2, "foc2");
}

static int dump2(const uint8_t *data, uint32_t len)
{
    return dump_data(data, len, &foccd1, "foc1");
}

int focc_init()
{
    drv = qdrv_get();
    pid_init(&focc.focc1.pid_iqloop, 15, 1.5, 0, 30);
    pid_init(&focc.focc1.pid_idloop, 15, 1.5, 0, 30);
    pid_init(&focc.focc1.pid_vloop, 0.001, 0.0001, 0.01, 150);
    pid_integ_sep_init(&focc.focc1.pid_ploop, 15, 0, 15, 10, 7200);
    focc.focc1.acc = 30;
    focc.focc1.position = 0;

    lpf_1st_init(&focc.focc1.oiqlpf, 2000, 21000);
    lpf_1st_init(&focc.focc1.oidlpf, 2000, 21000);
    lpf_1st_init(&focc.focc1.ovlpf, 100, 4000);
    lpf_1st_init(&focc.focc1.oplpf, 100, 1000);

    rms_init(&focc.focc1.eiq_rms, 2000);
    rms_init(&focc.focc1.eid_rms, 2000);
    rms_init(&focc.focc1.ev_rms, 500);
    rms_init(&focc.focc1.ep_rms, 100);

    pid_init(&focc.focc2.pid_iqloop, 15, 1.5, 0, 50);
    pid_init(&focc.focc2.pid_idloop, 15, 1.5, 0, 50);
    pid_init(&focc.focc2.pid_vloop, 0.001, 0.0001, 0.01, 150);
    pid_integ_sep_init(&focc.focc2.pid_ploop, 15, 0, 15, 10, 7200);
    focc.focc2.acc = 30;
    focc.focc2.position = 0;

    lpf_1st_init(&focc.focc2.oiqlpf, 2000, 21000);
    lpf_1st_init(&focc.focc2.oidlpf, 2000, 21000);
    lpf_1st_init(&focc.focc2.ovlpf, 100, 4000);
    lpf_1st_init(&focc.focc2.oplpf, 100, 1000);

    rms_init(&focc.focc2.eiq_rms, 2000);
    rms_init(&focc.focc2.eid_rms, 2000);
    rms_init(&focc.focc2.ev_rms, 100);
    rms_init(&focc.focc2.ep_rms, 100);

    foccd_init(&foccd1, dump1);
    foccd_init(&foccd2, dump2);

    qterm_attach(&cmd_focc1, "focc1", cmd_focc1_hdl, "foc1 controller commands");
    qterm_attach(&cmd_focc2, "focc2", cmd_focc2_hdl, "foc2 controller commands");
    qterm_attach(&cmd_focd1, "focd1", cmd_focd1_hdl, "foc1 debugger commands");
    qterm_attach(&cmd_focd2, "focd2", cmd_focd2_hdl, "foc2 debugger commands");

    return 0;
}

#define _FOCC_NULL ((void *)0)

static inline FoccDebugger *_get_foccd(FoccObj *foccobj)
{
    if(foccobj == &focc.focc1) {
        return &foccd1;
    } else {
        return &foccd2;
    }
}

static inline void update_time(FoccTime *time)
{
    time->now = bsp_ktime();
    if(time->last == 0) {
        time->last = time->now;
    }
}

static inline qfp_t err_limit(qfp_t err, qfp_t lim)
{
    if(lim == 0) {
        return err;
    }

    if(err > lim) {
        return lim;
    } else if(err < -lim) {
        return -lim;
    } else {
        return err;
    }
}

QFocError focc_iqd_loop(FoccObj *foccobj, const QFocObj *foc, QFocOutput *output)
{
    if(!foccobj || !foc || !output) {
        return QFOC_ERR_OBJ_NOT_FOUND;
    }

    update_time(&foccobj->time_iloop);

    FoccDebugger *foccd = _get_foccd(foccobj);

    qfp_t wave_output = 0;
    qfp_t iqref = foc->iqref;
    qfp_t idref = foc->idref;
    if(foccd_output(foccd, FOCC_DLOOP_IQD, &wave_output)) {
        iqref = wave_output;
        idref = 0;
        qfp_t dump[4] = { iqref, foc->iq, idref, foc->id };
        focd_dump(&foccd->focd, (uint8_t *)&dump, sizeof(dump));
    }

    qfp_t eiq = iqref - foc->iq;
    qfp_t eid = idref - foc->id;

    rms_update(&foccobj->eiq_rms, eiq);
    rms_update(&foccobj->eid_rms, eid);

    /* current input limit */
    eiq = err_limit(eiq, foccobj->eilim);
    eid = err_limit(eid, foccobj->eilim);

    qfp_t qref = pid_calc(&foccobj->pid_iqloop, eiq, PID_NONE);
    qfp_t dref = pid_calc(&foccobj->pid_idloop, eid, PID_NONE);
    output->vq = lpf_1st_calc(&foccobj->oiqlpf, qref);
    output->vd = lpf_1st_calc(&foccobj->oidlpf, dref);

    foccobj->iloop_calc_us = foccobj->time_iloop.now - foccobj->time_iloop.last;
    foccobj->time_iloop.last = foccobj->time_iloop.now;
    return QFOC_ERR_NONE;
}

QFocError focc_vloop(FoccObj *foccobj, const QFocObj *foc, QFocOutput *output)
{
    if(!foccobj || !foc || !output) {
        return QFOC_ERR_OBJ_NOT_FOUND;
    }
    update_time(&foccobj->time_vloop);

    FoccDebugger *foccd = _get_foccd(foccobj);

    qfp_t wave_output = 0;
    qfp_t vref = foc->vref;
    if(foccd_output(foccd, FOCC_DLOOP_V, &wave_output)) {
        vref = wave_output;
        qfp_t dump[2] = { vref, foc->vel };
        focd_dump(&foccd->focd, (uint8_t *)&dump, sizeof(dump));
    }

    qfp_t err = vref - foc->vel;
    rms_update(&foccobj->ev_rms, err);

    err = err_limit(err, foccobj->evlim);

    qfp_t ref = pid_calc(&foccobj->pid_vloop, err, PID_NONE);

    output->iqref = lpf_1st_calc(&foccobj->ovlpf, ref);
    output->idref = 0;
    output->to = QFOC_OUT_TO_ILOOP;
    foccobj->vloop_calc_us = foccobj->time_vloop.now - foccobj->time_vloop.last;
    foccobj->time_vloop.last = foccobj->time_vloop.now;
    return QFOC_ERR_NONE;
}

QFocError focc_ploop(FoccObj *foccobj, const QFocObj *foc, QFocOutput *output)
{
    if(!foccobj || !foc || !output) {
        return QFOC_ERR_OBJ_NOT_FOUND;
    }
    update_time(&foccobj->time_ploop);

    FoccDebugger *foccd = _get_foccd(foccobj);

    qfp_t wave_output = 0;
    qfp_t pref = foc->pref;
    if(foccd_output(foccd, FOCC_DLOOP_P, &wave_output)) {
        pref = wave_output + foccobj->origin;
        qfp_t dump[2] = { pref, foccobj->position };
        focd_dump(&foccd->focd, (uint8_t *)&dump, sizeof(dump));
    }

    pref = (pref < 0) ? (pref + 360) : pref;
    qfp_t p = foccobj->position;
    qfp_t err = pref - p;

    err = err_limit(err, foccobj->eplim);

    rms_update(&foccobj->ep_rms, err);

    qfp_t ref = pid_integ_sep_calc(&foccobj->pid_ploop, err, PID_NONE);

    if(foccobj->acc != QFOC_NO_LIMIT) {
        ref = foc->vref + foccobj->acc * (ref > foc->vref ? 1 : -1);
    }

    output->vref = lpf_1st_calc(&foccobj->oplpf, ref);
    output->to = QFOC_OUT_TO_VLOOP;
    foccobj->ploop_calc_us = foccobj->time_ploop.now - foccobj->time_ploop.last;
    foccobj->time_ploop.last = foccobj->time_ploop.now;
    return QFOC_ERR_NONE;
}

int focc_time_clr(FoccObj *foccobj)
{
    if(!foccobj) {
        return -1;
    }
    foccobj->time_iloop.last = 0;
    foccobj->time_vloop.last = 0;
    foccobj->time_ploop.last = 0;
    return 0;
}

int focc_controller_clr(FoccObj *foccobj)
{
    if(!foccobj) {
        return -1;
    }
    pid_clr(&foccobj->pid_iqloop);
    pid_clr(&foccobj->pid_idloop);
    pid_clr(&foccobj->pid_vloop);
    pid_clr(&foccobj->pid_ploop);

    foccobj->oiqlpf.y_k1 = 0;
    foccobj->oidlpf.y_k1 = 0;
    foccobj->ovlpf.y_k1 = 0;
    foccobj->oplpf.y_k1 = 0;

    return 0;
}

Focc *focc_get(void)
{
    return &focc;
}

static int args_oilpf_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" fc: %f\r\n", foccobj->oiqlpf.fc);
    } else if(argc == 2) {
        qfp_t fc = atof(argv[1]);
        lpf_1st_fc_set(&foccobj->oiqlpf, fc);
        lpf_1st_fc_set(&foccobj->oidlpf, fc);
    } else {
        return -1;
    }
    return 0;
}

static int args_ovlpf_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" fc: %f\r\n", foccobj->ovlpf.fc);
    } else if(argc == 2) {
        qfp_t fc = atof(argv[1]);
        lpf_1st_fc_set(&foccobj->ovlpf, fc);
    } else {
        return -1;
    }
    return 0;
}

static int args_oplpf_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" fc: %f\r\n", foccobj->oplpf.fc);
    } else if(argc == 2) {
        qfp_t fc = atof(argv[1]);
        lpf_1st_fc_set(&foccobj->oplpf, fc);
    } else {
        return -1;
    }
    return 0;
}

static int args_oilim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->pid_iqloop.olimit);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        foccobj->pid_iqloop.y_k1 = 0;
        foccobj->pid_idloop.y_k1 = 0;
        foccobj->pid_iqloop.olimit = lim;
        foccobj->pid_idloop.olimit = lim;
    } else {
        return -1;
    }
    return 0;
}

static int args_ovlim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->pid_vloop.olimit);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        foccobj->pid_vloop.y_k1 = 0;
        foccobj->pid_vloop.olimit = lim;
    } else {
        return -1;
    }
    return 0;
}

static int args_oplim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->pid_ploop.olimit);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        foccobj->pid_ploop.y_k1 = 0;
        foccobj->pid_ploop.olimit = lim;
    } else {
        return -1;
    }
    return 0;
}

static int args_eilim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->eilim);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        if(lim > 0) {
            foccobj->eilim = lim;
        } else {
            QSH(" limit must be positive\r\n");
        }
    } else {
        return -1;
    }
    return 0;
}

static int args_evlim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->evlim);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        if(lim > 0) {
            foccobj->evlim = lim;
        } else {
            QSH(" limit must be positive\r\n");
        }
    } else {
        return -1;
    }
    return 0;
}

static int args_eplim_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" limit: %f\r\n", foccobj->eplim);
    } else if(argc == 2) {
        qfp_t lim = atof(argv[1]);
        if(lim > 0) {
            foccobj->eplim = lim;
        } else {
            QSH(" limit must be positive\r\n");
        }
    } else {
        return -1;
    }
    return 0;
}

static int args_vff_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" k: %f\r\n", foccobj->vff_coeff);
    } else if(argc == 2) {
        foccobj->vff_coeff = atof(argv[1]);
    } else {
        return -1;
    }
    return 0;
}

static int args_pff_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" k: %f\r\n", foccobj->pff_coeff);
    } else if(argc == 2) {
        foccobj->pff_coeff = atof(argv[1]);
    } else {
        return -1;
    }
    return 0;
}

static int args_ipid_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f\r\n", foccobj->pid_iqloop.kp, foccobj->pid_iqloop.ki, foccobj->pid_iqloop.kd);
    } else if(argc == 4) {
        pid_clr(&foccobj->pid_iqloop);
        pid_param_set(&foccobj->pid_iqloop, atof(argv[1]), atof(argv[2]), atof(argv[3]));
        pid_param_set(&foccobj->pid_idloop, atof(argv[1]), atof(argv[2]), atof(argv[3]));
    } else {
        return -1;
    }
    return 0;
}

static int args_vpid_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f\r\n", foccobj->pid_vloop.kp, foccobj->pid_vloop.ki, foccobj->pid_vloop.kd);
    } else if(argc == 4) {
        pid_clr(&foccobj->pid_vloop);
        pid_param_set(&foccobj->pid_vloop, atof(argv[1]), atof(argv[2]), atof(argv[3]));
    } else {
        return -1;
    }
    return 0;
}

static int args_ppid_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f\r\n", foccobj->pid_ploop.kp, foccobj->pid_ploop.ki, foccobj->pid_ploop.kd);
    } else if(argc == 4) {
        pid_clr(&foccobj->pid_ploop);
        pid_param_set(&foccobj->pid_ploop, atof(argv[1]), atof(argv[2]), atof(argv[3]));
    } else {
        return -1;
    }
    return 0;
}

static int args_vppid_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" kp: %f, ki: %f, kd: %f\r\n", foccobj->pid_vloop.kp, foccobj->pid_vloop.ki, foccobj->pid_vloop.kd);
    } else if(argc == 4) {
        pid_clr(&foccobj->pid_vloop);
        pid_param_set(&foccobj->pid_vloop, atof(argv[1]), atof(argv[2]), atof(argv[3]));
    } else {
        return -1;
    }
    return 0;
}

static int args_acc_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" acc: %f\r\n", foccobj->acc);
    } else if(argc == 2) {
        qfp_t acc = atof(argv[1]);
        if(acc > 0) {
            foccobj->acc = atof(argv[1]);
        } else {
            QSH(" acc must be positive\r\n");
        }
    } else {
        return -1;
    }
    return 0;
}

static QCliArgsEntry args_focc_table[] = {
    {"oilpf", 1, 2, args_oilpf_hdl, "<fc>"},
    {"ovlpf", 1, 2, args_ovlpf_hdl, "<fc>"},
    {"oplpf", 1, 2, args_oplpf_hdl, "<fc>"},
    {"oilim", 1, 2, args_oilim_hdl, "<lim>"},
    {"ovlim", 1, 2, args_ovlim_hdl, "<lim>"},
    {"oplim", 1, 2, args_oplim_hdl, "<lim>"},
    {"eilim", 1, 2, args_eilim_hdl, "<lim>"},
    {"evlim", 1, 2, args_evlim_hdl, "<lim>"},
    {"eplim", 1, 2, args_eplim_hdl, "<lim>"},
    {"vff", 1, 2, args_vff_hdl, "<k>"},
    {"pff", 1, 2, args_pff_hdl, "<k>"},
    {"ipid", 1, 4, args_ipid_hdl, "<kp> <ki> <kd>"},
    {"vpid", 1, 4, args_vpid_hdl, "<kp> <ki> <kd>"},
    {"ppid", 1, 4, args_ppid_hdl, "<kp> <ki> <kd>"},
    {"vppid", 1, 4, args_vppid_hdl, "<kp> <ki> <kd>"},
    {"acc", 1, 2, args_acc_hdl, "<acc>"},
};

int cmd_focc_hdl(int argc, char **argv)
{
    if(argc == 1) {
        QSH(" erms eiq: %f, eid: %f, ev: %f, ep: %f\r\n", foccobj->eiq_rms, foccobj->eid_rms, foccobj->ev_rms, foccobj->ep_rms);
        QSH(" us: iloop: %u, vloop: %u, ploop: %u\r\n", foccobj->iloop_calc_us, foccobj->vloop_calc_us, foccobj->ploop_calc_us);
        return 0;
    }
    if(ISARG(argv[1], "?") && (argc == 2)) {
        return qterm_help(args_focc_table, sizeof(args_focc_table));
    }
    return qcli_args_handle(argc, argv, args_focc_table, sizeof(args_focc_table));
}

static NameTable qwave_type_table[] = {
    {QWAVE_TYPE_SINE, "sine"},
    {QWAVE_TYPE_TRIANGLE, "tri"},
    {QWAVE_TYPE_SAWTOOTH, "saw"},
    {QWAVE_TYPE_ANTSAWTOOTH, "ant"},
    {QWAVE_TYPE_NOISE, "noise"},
    {QWAVE_TYPE_SQUARE, "sqr"},
};

static int args_off_hdl(int argc, char **argv)
{
    UNUSED(argc);
    UNUSED(argv);
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(foccd == &foccd1) {
        qterm_call("foc1 ena 0");
    } else {
        qterm_call("foc2 ena 0");
    }
    foccd->dloop = FOCC_DLOOP_NONE;
    focd_amp_set(&foccd->focd, 0);
    focd_ena_set(&foccd->focd, FOCD_OFF);
    foccd->focd.is_dump = false;
    return 0;
}

static int args_loop_hdl(int argc, char **argv)
{
    UNUSED(argc);
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(ISARG(argv[1], "i")) {
        char buf[32] = { 0 };
        if(foccd == &foccd1) {
            snprintf(buf, sizeof(buf), "foc1 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc1 cmod i");
            qterm_call(buf);
        } else {
            snprintf(buf, sizeof(buf), "foc2 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc2 cmod i");
            qterm_call(buf);
        }
        foccd->dloop = FOCC_DLOOP_IQD;
        focd_amp_set(&foccd->focd, 0);
        focd_fs_set(&foccd->focd, 21000);
        focd_ena_set(&foccd->focd, FOCD_ON);
        foccd->focd.is_dump = true;
    } else if(ISARG(argv[1], "v")) {
        char buf[32] = { 0 };
        if(foccd == &foccd1) {
            snprintf(buf, sizeof(buf), "foc1 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc1 cmod v");
            qterm_call(buf);
        } else {
            snprintf(buf, sizeof(buf), "foc2 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc2 cmod v");
            qterm_call(buf);
        }
        foccd->dloop = FOCC_DLOOP_V;
        focd_amp_set(&foccd->focd, 0);
        focd_fs_set(&foccd->focd, 4200);
        focd_ena_set(&foccd->focd, FOCD_ON);
        foccd->focd.is_dump = true;
    } else if(ISARG(argv[1], "p")) {
        char buf[32] = { 0 };
        if(foccd == &foccd1) {
            focc.focc1.origin = drv->foc1.pos;
            snprintf(buf, sizeof(buf), "foc1 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc1 cmod p");
            qterm_call(buf);
        } else {
            focc.focc2.origin = drv->foc2.pos;
            snprintf(buf, sizeof(buf), "foc2 ena 1");
            qterm_call(buf);
            snprintf(buf, sizeof(buf), "foc2 cmod p");
            qterm_call(buf);
        }
        foccd->dloop = FOCC_DLOOP_P;
        focd_amp_set(&foccd->focd, 0);
        focd_fs_set(&foccd->focd, 1000);
        focd_ena_set(&foccd->focd, FOCD_ON);
        foccd->focd.is_dump = true;
    } else {
        return -1;
    }
    return 0;
}

static int args_wave_hdl(int argc, char **argv)
{
    UNUSED(argc);
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    char buf[32] = { 0 };
    if(foccd == &foccd1) {
        snprintf(buf, sizeof(buf), "foc1 ena 0");
        qterm_call(buf);
    } else {
        snprintf(buf, sizeof(buf), "foc2 ena 0");
        qterm_call(buf);
    }
    if(ISARG(argv[1], "sin")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_SINE);
    } else if(ISARG(argv[1], "tri")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_TRIANGLE);
    } else if(ISARG(argv[1], "saw")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_SAWTOOTH);
    } else if(ISARG(argv[1], "ant")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_ANTSAWTOOTH);
    } else if(ISARG(argv[1], "noise")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_NOISE);
    } else if(ISARG(argv[1], "sqr")) {
        focd_signal_set(&foccd->focd, QWAVE_TYPE_SQUARE);
    } else {
        return -1;
    }
    return 0;
}

static int args_amp_hdl(int argc, char **argv)
{
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(argc == 1) {
        QSH(" amp: %f\r\n", foccd->focd.wave.amp);
        return 0;
    }
    focd_amp_set(&foccd->focd, atof(argv[1]));
    return 0;
}

static int args_frq_hdl(int argc, char **argv)
{
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(argc == 1) {
        QSH(" frq: %f\r\n", foccd->focd.wave.frq);
        return 0;
    }
    focd_frq_set(&foccd->focd, atof(argv[1]));
    return 0;
}

static int args_dump_hdl(int argc, char **argv)
{
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(argc == 1) {
        QSH(" isdump: %d\r\n", foccd->focd.is_dump);
        return 0;
    }
    if(ISARG(argv[1], "on")) {
        foccd->focd.is_dump = true;
    } else if(ISARG(argv[1], "off")) {
        foccd->focd.is_dump = false;
    } else {
        return -1;
    }
    return 0;
}

static QCliArgsEntry args_focd_table[] = {
    {"off", 1, 1, args_off_hdl, "stop debug"},
    {"loop", 1, 2, args_loop_hdl, "i/v/p"},
    {"wave", 1, 2, args_wave_hdl, "sin/tri/saw/ant/noise/sqr"},
    {"amp", 1, 2, args_amp_hdl, "<amp>"},
    {"frq", 1, 2, args_frq_hdl, "<frq>"},
    {"dump", 1, 2, args_dump_hdl, "on/off"},
};

int cmd_focd_hdl(int argc, char **argv)
{
    FoccDebugger *foccd = foccobj == &focc.focc1 ? &foccd1 : &foccd2;
    if(argc == 1) {
        QSH(" ena: %d\r\n", foccd->focd.ena);
        QSH(" wave: %s\r\n", qwave_type_table[foccd->focd.wave.type].name);
        QSH(" amp: %f\r\n", foccd->focd.wave.amp);
        QSH(" freq: %f\r\n", foccd->focd.wave.frq);
        QSH(" foccd->focd.is_dump: %d\r\n", foccd->focd.is_dump);

        QSH(" time: %f <%04x>\r\n", foccd->focd.wave.t, foccd->focd.wave.t);
        QSH(" fs: %f\r\n", foccd->focd.wave.fs);
        QSH(" ts: %f\r\n", foccd->focd.wave.ts);
        QSH(" period: %f\r\n", foccd->focd.wave.period);
        return 0;
    } else if(ISARG(argv[1], "?") && (argc == 2)) {
        return qterm_help(args_focd_table, sizeof(args_focd_table));
    } else {
        return qcli_args_handle(argc, argv, args_focd_table, sizeof(args_focd_table));
    }
}
